#include "main.h"

#define SlaveNum 1

#define AnaInSlavePos 0, 0
#define Beckhoff_AX58100  0x00000009,0x26483052


union RV_TypeConvert
{
    float to_float;
    int to_int;
    unsigned int to_uint;
    uint8_t buf[4];
} rv_type_convert;

ec_pdo_entry_info_t slave_0_pdo_entries_out[68];

ec_pdo_entry_info_t slave_0_pdo_entries_in[68];

ec_pdo_entry_info_t slave_0_pdo_entries_out_pdo[] = {
    {0x6020, 0x01, 1},
    {0x6020, 0x02, 1},
    {0x6020, 0x03, 2},
    {0x6020, 0x05, 2},
    {0x0000, 0x00, 8},
    {0x1802, 0x07, 1},
    {0x1802, 0x09, 1},
    {0x6020, 0x0b, 16},
    {0x6020, 0x0c, 16},
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1601, 68, slave_0_pdo_entries_out},
    {0x1a00, 68, slave_0_pdo_entries_in},
    {0x1a02, 9, slave_0_pdo_entries_out_pdo},
};
ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, 0, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, 0, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_0_pdos, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 2, slave_0_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

EtherCAT_Msg tx_msg[SlaveNum];
EtherCAT_Msg rx_msg[SlaveNum];

OD_Motor_Msg rv_motor_msg[SlaveNum][6];

PDO_Struct pdo_struct[SlaveNum];
ec_pdo_entry_reg_t domain_regs_in[68 * 2 * SlaveNum + 1];


// 手动初始化PDO配置数组
void init_pdo_entries(ec_pdo_entry_info_t* pdo_entries_out, ec_pdo_entry_info_t* pdo_entries_in)
{
    int subindex, bit_length;

    for (int i = 0; i < 68; i++)
    {
        subindex = (i + 1);
        bit_length = ((i - 2) % 11) ? 8 : 32;

        pdo_entries_out[i] = (ec_pdo_entry_info_t){0x7010, subindex, bit_length};
        pdo_entries_in[i] = (ec_pdo_entry_info_t){0x6000, subindex, bit_length};
    }
}


// 手动初始化PDO映射
void init_domain_regs(ec_pdo_entry_reg_t* domain_regs)
{
    int i = 0;

    for (int slave = 0; slave < SlaveNum; slave++)
    {
        domain_regs[i++] = (ec_pdo_entry_reg_t){
            0, slave, Beckhoff_AX58100, 0x7010, 1, &pdo_struct[slave].OUT_NOTOR_NUM
        };
        domain_regs[i++] = (ec_pdo_entry_reg_t){0, slave, Beckhoff_AX58100, 0x7010, 2, &pdo_struct[slave].OUT_CAN_IDE};

        // 为每个电机初始化输出PDO映射
        for (int motor = 0; motor < 6; motor++)
        {
            domain_regs[i++] = (ec_pdo_entry_reg_t){
                0, slave, Beckhoff_AX58100, 0x7010, 3 + motor * 11, &pdo_struct[slave].OUT_MOTOR[motor].ID
            };
            domain_regs[i++] = (ec_pdo_entry_reg_t){
                0, slave, Beckhoff_AX58100, 0x7010, 4 + motor * 11, &pdo_struct[slave].OUT_MOTOR[motor].RTR
            };
            domain_regs[i++] = (ec_pdo_entry_reg_t){
                0, slave, Beckhoff_AX58100, 0x7010, 5 + motor * 11, &pdo_struct[slave].OUT_MOTOR[motor].DLC
            };
            for (int data_idx = 0; data_idx < 8; data_idx++)
            {
                domain_regs[i++] = (ec_pdo_entry_reg_t){
                    0, slave, Beckhoff_AX58100, 0x7010, 6 + data_idx + motor * 11,
                    &pdo_struct[slave].OUT_MOTOR[motor].DATA[data_idx]
                };
            }
        }
    }
    for (int slave = 0; slave < SlaveNum; slave++)
    {
        domain_regs[i++] = (ec_pdo_entry_reg_t){0, slave, Beckhoff_AX58100, 0x6000, 1, &pdo_struct[slave].IN_MOTOR_NUM};
        domain_regs[i++] = (ec_pdo_entry_reg_t){0, slave, Beckhoff_AX58100, 0x6000, 2, &pdo_struct[slave].IN_MOTOR_IDE};

        // 为每个电机初始化输入PDO映射
        for (int motor = 0; motor < 6; motor++)
        {
            domain_regs[i++] = (ec_pdo_entry_reg_t){
                0, slave, Beckhoff_AX58100, 0x6000, 3 + motor * 11, &pdo_struct[slave].IN_MOTOR[motor].ID
            };
            domain_regs[i++] = (ec_pdo_entry_reg_t){
                0, slave, Beckhoff_AX58100, 0x6000, 4 + motor * 11, &pdo_struct[slave].IN_MOTOR[motor].RTR
            };
            domain_regs[i++] = (ec_pdo_entry_reg_t){
                0, slave, Beckhoff_AX58100, 0x6000, 5 + motor * 11, &pdo_struct[slave].IN_MOTOR[motor].DLC
            };
            for (int data_idx = 0; data_idx < 8; data_idx++)
            {
                domain_regs[i++] = (ec_pdo_entry_reg_t){
                    0, slave, Beckhoff_AX58100, 0x6000, 6 + data_idx + motor * 11,
                    &pdo_struct[slave].IN_MOTOR[motor].DATA[data_idx]
                };
            }
        }
    }

    // 结束标记
    domain_regs[i] = (ec_pdo_entry_reg_t){0xff};
}

/*****************************************************************************/
void check_domain_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domain, &ds);

    if (!(ds.working_counter && ds.wc_state))
        return;

    if (ds.working_counter != domain_state.working_counter)
    {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domain_state.wc_state)
    {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domain_state = ds;
}

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding)
    {
        printf("%u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states)
    {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up)
    {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/****************************************************************************/
static void inc_period(struct period_info* pinfo)
{
    pinfo->next_period.tv_nsec += pinfo->period_ns;
    while (pinfo->next_period.tv_nsec >= 1000000000)
    {
        /* timespec nsec overflow */
        pinfo->next_period.tv_sec++;
        pinfo->next_period.tv_nsec -= 1000000000;
    }
}

/****************************************************************************/
static void periodic_task_init(struct period_info* pinfo)
{
    /* for simplicity, hardcoding a 1 ms period */
    pinfo->period_ns = 1000000;
    /* find timestamp to first run */
    clock_gettime(1, &(pinfo->next_period));
}

static void wait_rest_of_period(struct period_info* pinfo)
{
    inc_period(pinfo);
    /* for simplicity, ignoring possibilities of signal wakes */
    clock_nanosleep(1, 1, &pinfo->next_period, NULL);
}


static void ethercat_data_exchange()
{
    for (int slave = 0; slave < SlaveNum; slave++) {
        rx_msg[slave].can_ide = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR_IDE);
        rx_msg[slave].motor_num = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR_NUM);

        EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_CAN_IDE, tx_msg[slave].can_ide);
        EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_NOTOR_NUM, tx_msg[slave].motor_num );

        for(int i = 0; i < 6; i++) {
            rx_msg[slave].motor[i].id = EC_READ_U32(domain_pd+pdo_struct[slave].IN_MOTOR[i].ID);
            rx_msg[slave].motor[i].rtr = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR[i].RTR);
            rx_msg[slave].motor[i].dlc = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR[i].DLC);

            EC_WRITE_U32(domain_pd+pdo_struct[slave].OUT_MOTOR[i].ID, tx_msg[slave].motor[i].id);
            EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_MOTOR[i].RTR, tx_msg[slave].motor[i].rtr);
            EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_MOTOR[i].DLC, tx_msg[slave].motor[i].dlc);

            for(int j = 0; j < 8; j++) {
                rx_msg[slave].motor[i].data[j] = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR[i].DATA[j]);
                
                EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_MOTOR[i].DATA[j], tx_msg[slave].motor[i].data[j]);
            }
        }
    }
}

static void ethercat_data_get(void) {

    for (int slave = 0; slave < SlaveNum; slave++) {
        rx_msg[slave].can_ide = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR_IDE);
        rx_msg[slave].motor_num = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR_NUM);        

        for(int i = 0; i < 6; i++) {
            rx_msg[slave].motor[i].id = EC_READ_U32(domain_pd+pdo_struct[slave].IN_MOTOR[i].ID);
            rx_msg[slave].motor[i].rtr = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR[i].RTR);
            rx_msg[slave].motor[i].dlc = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR[i].DLC);

            for(int j = 0; j < 8; j++) {
                rx_msg[slave].motor[i].data[j] = EC_READ_U8(domain_pd+pdo_struct[slave].IN_MOTOR[i].DATA[j]);
            }
        }
    }
}

static void ethercat_data_set(void) { 
    for (int slave = 0; slave < SlaveNum; slave++) {

        EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_CAN_IDE, tx_msg[slave].can_ide);
        EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_NOTOR_NUM, tx_msg[slave].motor_num );

        for(int i = 0; i < 6; i++) {

            EC_WRITE_U32(domain_pd+pdo_struct[slave].OUT_MOTOR[i].ID, tx_msg[slave].motor[i].id);
            EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_MOTOR[i].RTR, tx_msg[slave].motor[i].rtr);
            EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_MOTOR[i].DLC, tx_msg[slave].motor[i].dlc);

            for(int j = 0; j < 8; j++) {
                EC_WRITE_U8(domain_pd+pdo_struct[slave].OUT_MOTOR[i].DATA[j], tx_msg[slave].motor[i].data[j]);
            }
        }
    }
}

static void do_ec_task()
{

    ecrt_master_receive(master);
    ecrt_domain_process(domain);

    ethercat_data_get();

    check_domain_state();
    check_master_state();

    set_motor_speed(tx_msg, 1, 1, 5, 10, 2);
    set_motor_speed(tx_msg, 2, 2, 5, 10, 2);
    set_motor_speed(tx_msg, 3, 3, 5, 10, 1);
    set_motor_speed(tx_msg, 4, 4, 5, 10, 1);
    set_motor_speed(tx_msg, 5, 5, 5, 10, 1);
    set_motor_speed(tx_msg, 6, 6, 5, 10, 1);

    RV_can_data_repack_all(rx_msg, comm_ack, rv_motor_msg, SlaveNum, 0);

    ethercat_data_set();

    ecrt_domain_queue(domain);
    ecrt_master_send(master);
}

void* simple_syclic_task(void* data)
{
    printf("Started cyclic thread.\n");
    struct period_info pinfo;
    periodic_task_init(&pinfo);

    while (1)
    {
        do_ec_task();

        wait_rest_of_period(&pinfo);
    }
    return 0;
}

int init_ethercat()
{
    init_pdo_entries(slave_0_pdo_entries_out, slave_0_pdo_entries_in);
    init_domain_regs(domain_regs_in);

    ec_slave_config_t* sc[SlaveNum];
    master = ecrt_request_master(0);
    if (!master)
    {
        return -1;
    }
    domain = ecrt_master_create_domain(master);
    if (!domain)
    {
        return -1;
    }
    for (int slave = 0; slave < SlaveNum; slave++)
    {
        if (!(sc[slave] = ecrt_master_slave_config(master, 0, slave, Beckhoff_AX58100)))
        {
            fprintf(stderr, "Failed to get slave %d configuration.\n", slave);
            return -1;
        }
        printf("Configuring PDOs for slave %d...\n", slave);
        if (ecrt_slave_config_pdos(sc[slave], EC_END, slave_0_syncs))
        {
            fprintf(stderr, "Failed to configure PDOs for slave %d.\n", slave);
            return -1;
        }
    }

    if (ecrt_domain_reg_pdo_entry_list(domain, domain_regs_in))
    {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    printf("Activating master...\n");
    if (ecrt_master_activate(master))
    {
        return -1;
    }
    if (!(domain_pd = ecrt_domain_data(domain)))
    {
        return -1;
    }

    

    return 0;
}

int main()
{
    struct sched_param param;
    pthread_attr_t attr;
    pthread_t thread;
    int ret;
    ret = init_ethercat();
    if (ret)
    {
        printf("Failed to init EtherCAT master.\n");
        exit(-2);
    }
    /* Lock memory */
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
    {
        printf("mlockall() failed: %m\n");
        exit(-2);
    }
    /* Initialize pthread attributes (default values) */
    ret = pthread_attr_init(&attr);
    if (ret)
    {
        printf("init pthread attributes failed\n");
        goto out;
    }
    /* Set a specific stack size  */
    ret = pthread_attr_setstacksize(&attr, 16384 + 1);
    if (ret)
    {
        printf("pthread setstacksize failed\n");
        goto out;
    }
    /* Set scheduler policy and priority of pthread */
    ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    if (ret)
    {
        printf("pthread setschedpolicy failed\n");
        goto out;
    }
    param.sched_priority = 80;
    ret = pthread_attr_setschedparam(&attr, &param);
    if (ret)
    {
        printf("pthread setschedparam failed\n");
        goto out;
    }
    /* Use scheduling parameters of attr */
    ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    if (ret)
    {
        printf("pthread setinheritsched failed\n");
        goto out;
    }
    /* Create a pthread with specified attributes */
    ret = pthread_create(&thread, &attr, simple_syclic_task, NULL);
    if (ret)
    {
        printf("create pthread failed: %s\n", strerror(ret));
        goto out;
    }
    ret = pthread_setname_np(thread, "ethercat-rt");
    if (ret)
    {
        printf("failed to set thread name\n");
    }
    /* Join the thread and wait until it is done */
    ret = pthread_join(thread, NULL);
    if (ret)
    {
        printf("join pthread failed: %m\n");
    }
out:
    return ret;
}
